import math
import sys
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
import DepthControl
import ControlBase
import NetWorkBase
import MFAC
import threading
import datetime
import time
import PDControl
import StateBase
import os
import random
from PyQt5.QtChart import QChart, QChartView, QLineSeries, QValueAxis, QSplineSeries
# import pyqtgraph as pg


class ServerApp(QWidget):
    state_signal = pyqtSignal(str)

    def __init__(self, *args, **kwargs):
        super(ServerApp, self).__init__(*args, **kwargs)
        self.send_th = None
        self.state = StateBase.State()
        self.TCP_client = None
        self.TCP_init = None
        self.send_str = None
        self.flag = True
        # self.control_dic = None
        self.control_dic = {'a_p': 10.0, 'a_i': 0.0, 'a_d': 1.0, 'd_p': 1, 'd_i': 1, 'd_d': 1, 'v_p': 5.0, 'v_i': 0.01, 'v_d': 1}
        self.PID_V = PDControl.PDControl('V')
        self.PID_V.control_dic = self.control_dic
        self.PID_V.set_pid()
        self.PID_A = PDControl.PDControl('A')
        self.PID_A.control_dic = self.control_dic
        self.PID_A.set_pid()
        self.PID_D = DepthControl.DeltaPID('D')

        self.initUI()
        self.Graph_box2()
        self.Graph_box1()

        self.get_th = threading.Thread(target=self.getMessage, args=())
        self.get_th.daemon = True
        self.get_th.start()
        self.get_th_1 = threading.Thread(target=self.getMessage_1, args=())
        self.get_th_1.daemon = True
        self.get_th_1.start()

    def initUI(self):
        self.setWindowTitle('Control')
        self.setFixedSize(1080, 900)
        self.V_edit = QLineEdit(self)
        self.V_edit.move(100, 10)
        self.V_edit.setFixedSize(100, 30)
        self.V_edit.setText('1')
        self.V_edit.setPlaceholderText("期望速度：只能输入数字")

        self.A_edit = QLineEdit(self)
        self.A_edit.move(100, 50)
        self.A_edit.setFixedSize(100, 30)
        self.A_edit.setPlaceholderText("期望艏向：只能输入数字")
        self.A_edit.setText('1')

        self.D_edit = QLineEdit(self)
        self.D_edit.move(100, 90)
        self.D_edit.setFixedSize(100, 30)
        self.D_edit.setText('1')
        self.D_edit.setPlaceholderText("期望深度：只能输入数字")

        self.V_btn = QPushButton(self)
        self.V_btn.move(15, 10)
        self.V_btn.setFixedSize(70, 30)
        self.V_btn.setText("速度")

        self.A_btn = QPushButton(self)
        self.A_btn.move(15, 50)
        self.A_btn.setFixedSize(70, 30)
        self.A_btn.setText("角度")

        self.D_btn = QPushButton(self)
        self.D_btn.move(15, 90)
        self.D_btn.setFixedSize(70, 30)
        self.D_btn.setText("深度")

        self.X_btn = QPushButton(self)
        self.X_btn.move(220, 50)
        self.X_btn.setText("速度\角度\深度确认")
        self.X_btn.clicked.connect(self.enter_V_A)

        self.net_btn = QPushButton(self)
        self.net_btn.move(900, 10)
        self.net_btn.setText("网络连接")
        self.net_btn.clicked.connect(self.net_connect)

        self.net_btn_send = QPushButton(self)
        self.net_btn_send.move(900, 50)
        self.net_btn_send.setText("发送")
        self.net_btn_send.clicked.connect(self.net_send)

        self.net_btn_stop_send = QPushButton(self)
        self.net_btn_stop_send.move(900, 90)
        self.net_btn_stop_send.setText("停止发送")
        self.net_btn_stop_send.clicked.connect(self.stop_send)

        self.test1 = QPushButton(self)
        self.test1.move(900, 130)
        self.test1.setText("测试")
        self.test1.clicked.connect(self.test)

        self.KP_edit = QLineEdit(self)
        self.KP_edit.move(100, 140)
        self.KP_edit.setFixedSize(100, 30)
        self.KP_edit.setText(str(self.control_dic['a_p']))
        self.KP_edit.setPlaceholderText("Kp：只能输入数字")

        self.KD_edit = QLineEdit(self)
        self.KD_edit.move(100, 180)
        self.KD_edit.setFixedSize(100, 30)
        self.KD_edit.setText(str(self.control_dic['a_d']))
        self.KD_edit.setPlaceholderText("Kd：只能输入数字")

        self.KP_btn = QPushButton(self)
        self.KP_btn.move(15, 140)
        self.KP_btn.setFixedSize(70, 30)
        self.KP_btn.setText("KP")

        self.KD_btn = QPushButton(self)
        self.KD_btn.move(15, 180)
        self.KD_btn.setFixedSize(70, 30)
        self.KD_btn.setText("KD")

        self.KD_btn = QPushButton(self)
        self.KD_btn.move(220, 160)
        self.KD_btn.setText("Kd\Kp确认")
        self.KD_btn.clicked.connect(self.flash_PID)

        self.TextBox = QTextBrowser(self)
        self.TextBox.move(40, 700)
        self.TextBox.setFixedSize(1000, 150)
        self.TextBox.setText('log')

        self.state_show()


    def state_show(self):
        self.TextBox_x = QLineEdit(self)
        self.TextBox_x.move(400, 32)
        self.TextBox_x.setFixedSize(55, 25)

        self.TextBox_y = QLineEdit(self)
        self.TextBox_y.move(460, 32)
        self.TextBox_y.setFixedSize(55, 25)

        self.TextBox_z = QLineEdit(self)
        self.TextBox_z.move(520, 32)
        self.TextBox_z.setFixedSize(55, 25)

        self.TextBox_u = QLineEdit(self)
        self.TextBox_u.move(580, 32)
        self.TextBox_u.setFixedSize(55, 25)
        self.TextBox_u.setText("00.00")
        self.TextBox_u.setAlignment(Qt.AlignCenter)

        self.TextBox_v = QLineEdit(self)
        self.TextBox_v.move(640, 32)
        self.TextBox_v.setFixedSize(55, 25)
        self.TextBox_v.setText("00.00")
        self.TextBox_v.setAlignment(Qt.AlignCenter)

        self.TextBox_w = QLineEdit(self)
        self.TextBox_w.move(700, 32)
        self.TextBox_w.setFixedSize(55, 25)

        self.TextBox_pitch = QLineEdit(self)
        self.TextBox_pitch.move(400, 82)
        self.TextBox_pitch.setFixedSize(55, 25)

        self.TextBox_roll = QLineEdit(self)
        self.TextBox_roll.move(460, 82)
        self.TextBox_roll.setFixedSize(55, 25)

        self.TextBox_yaw = QLineEdit(self)
        self.TextBox_yaw.move(520, 82)
        self.TextBox_yaw.setFixedSize(55, 25)
        self.TextBox_yaw.setText("00.00")
        self.TextBox_yaw.setAlignment(Qt.AlignCenter)


        self.TextBox_p = QLineEdit(self)
        self.TextBox_p.move(580, 82)
        self.TextBox_p.setFixedSize(55, 25)

        self.TextBox_q = QLineEdit(self)
        self.TextBox_q.move(640, 82)
        self.TextBox_q.setFixedSize(55, 25)

        self.TextBox_r = QLineEdit(self)
        self.TextBox_r.move(700, 82)
        self.TextBox_r.setFixedSize(55, 25)

        self.TextBox_time = QLineEdit(self)
        self.TextBox_time.move(770, 62)
        self.TextBox_time.setFixedSize(120, 20)

        self.TextLabel_x = QLabel(self)
        self.TextLabel_x.move(400, 10)
        self.TextLabel_x.setFixedSize(55, 25)
        self.TextLabel_x.setText("X")
        self.TextLabel_x.setAlignment(Qt.AlignCenter)

        self.TextLabel_y = QLabel(self)
        self.TextLabel_y.move(460, 10)
        self.TextLabel_y.setFixedSize(55, 25)
        self.TextLabel_y.setText("Y")
        self.TextLabel_y.setAlignment(Qt.AlignCenter)

        self.TextLabel_z = QLabel(self)
        self.TextLabel_z.move(520, 10)
        self.TextLabel_z.setFixedSize(55, 25)
        self.TextLabel_z.setText("Z")
        self.TextLabel_z.setAlignment(Qt.AlignCenter)

        self.TextLabel_u = QLabel(self)
        self.TextLabel_u.move(580, 10)
        self.TextLabel_u.setFixedSize(55, 25)
        self.TextLabel_u.setText("u")
        self.TextLabel_u.setAlignment(Qt.AlignCenter)

        self.TextLabel_v = QLabel(self)
        self.TextLabel_v.move(640, 10)
        self.TextLabel_v.setFixedSize(55, 25)
        self.TextLabel_v.setText("v")
        self.TextLabel_v.setAlignment(Qt.AlignCenter)

        self.TextLabel_w = QLabel(self)
        self.TextLabel_w.move(700, 10)
        self.TextLabel_w.setFixedSize(55, 25)
        self.TextLabel_w.setText("w")
        self.TextLabel_w.setAlignment(Qt.AlignCenter)

        self.TextLabel_pitch = QLabel(self)
        self.TextLabel_pitch.move(400, 60)
        self.TextLabel_pitch.setFixedSize(55, 25)
        self.TextLabel_pitch.setText("pitch")
        self.TextLabel_pitch.setAlignment(Qt.AlignCenter)

        self.TextLabel_roll = QLabel(self)
        self.TextLabel_roll.move(460, 60)
        self.TextLabel_roll.setFixedSize(55, 25)
        self.TextLabel_roll.setText("roll")
        self.TextLabel_roll.setAlignment(Qt.AlignCenter)

        self.TextLabel_yaw = QLabel(self)
        self.TextLabel_yaw.move(520, 60)
        self.TextLabel_yaw.setFixedSize(55, 25)
        self.TextLabel_yaw.setText("yaw")
        self.TextLabel_yaw.setAlignment(Qt.AlignCenter)

        self.TextLabel_p = QLabel(self)
        self.TextLabel_p.move(580, 60)
        self.TextLabel_p.setFixedSize(55, 25)
        self.TextLabel_p.setText("p")
        self.TextLabel_p.setAlignment(Qt.AlignCenter)

        self.TextLabel_q = QLabel(self)
        self.TextLabel_q.move(640, 60)
        self.TextLabel_q.setFixedSize(55, 25)
        self.TextLabel_q.setText("q")
        self.TextLabel_q.setAlignment(Qt.AlignCenter)

        self.TextLabel_r = QLabel(self)
        self.TextLabel_r.move(700, 60)
        self.TextLabel_r.setFixedSize(55, 25)
        self.TextLabel_r.setText("r")
        self.TextLabel_r.setAlignment(Qt.AlignCenter)

        self.TextLabel_time = QLabel(self)
        self.TextLabel_time.move(800, 40)
        self.TextLabel_time.setFixedSize(100, 20)
        self.TextLabel_time.setText("time")


    def Graph_box1(self):
        self.Automatic_chart_view1 = QChartView(self)
        self.Automatic_chart_view1.setGeometry(550, 250, 500, 400)  # 设置图表视图位置和大小

        # 创建测试数据
        self.Automatic_data1 = []

        # 创建定时器，每隔一定时间触发更新函数
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.update_chart1)
        self.timer.start(1000)  # 每隔一毫秒触发一次

        self.chart = QChart()
        self.chart.setTitle('航向曲线图')

        self.axis_x = QValueAxis()
        self.axis_y = QValueAxis()

        # 设置坐标轴的范围
        self.axis_x.setRange(0, 10)
        self.axis_y.setRange(0, 10)

        # 设置图表到图表视图
        self.Automatic_chart_view1.setChart(self.chart)

        # 将坐标轴添加到图表上
        self.chart.setAxisX(self.axis_x)
        self.chart.setAxisY(self.axis_y)

    def update_chart1(self):
        # 模拟新数据的动态添加
        # new_data_point = (0, 10)
        velocity = float(self.TextBox_yaw.text())
        new_data_point = (len(self.Automatic_data1), velocity)
        # print("new_data_point:",new_data_point)
        self.Automatic_data1.append(new_data_point)
        self.Automatic_data1_50 = self.Automatic_data1[-50:]

        self.draw_chart1()

    def draw_chart1(self):
        # 清除旧的数据系列
        self.chart.removeAllSeries()

        # 创建数据系列
        series = QLineSeries()

        # 添加数据点
        for point in self.Automatic_data1_50:
            series.append(point[0], point[1])

        # 添加数据系列到图表上
        self.chart.addSeries(series)

        # 获取数据中的最大最小值
        x_values = [point[0] for point in self.Automatic_data1_50]
        y_values = [point[1] for point in self.Automatic_data1_50]

        # 设置坐标轴的范围（加上一些边界空间）
        self.axis_x.setRange(0, max(x_values))
        self.axis_y.setRange(min(y_values), max(y_values))

        # 更新图表
        self.Automatic_chart_view1.setChart(self.chart)


    def Graph_box2(self):
        self.Automatic_chart_view2 = QChartView(self)
        self.Automatic_chart_view2.setGeometry(25, 250, 500, 400)  # 设置图表视图位置和大小

        # 创建测试数据
        self.Automatic_data2 = []

        # 创建定时器，每隔一定时间触发更新函数
        self.timer2 = QTimer(self)
        self.timer2.timeout.connect(self.update_chart2)
        self.timer2.start(1000)  # 每隔一秒触发一次

        self.chart2 = QChart()
        self.chart2.setTitle('航速曲线图')

        self.axis_x2 = QValueAxis()
        self.axis_y2 = QValueAxis()

        # 设置坐标轴的范围
        self.axis_x2.setRange(0, 10)
        self.axis_y2.setRange(0, 10)

        # 设置图表到图表视图
        self.Automatic_chart_view2.setChart(self.chart2)

        # 将坐标轴添加到图表上
        self.chart2.setAxisX(self.axis_x2)
        self.chart2.setAxisY(self.axis_y2)

    def update_chart2(self):
        # 模拟新数据的动态添加
        velocity = math.hypot(float(self.TextBox_u.text()), float(self.TextBox_v.text()))
        new_data_point = (len(self.Automatic_data2), velocity)
        # print("new_data_point:",new_data_point)
        self.Automatic_data2.append(new_data_point)
        self.Automatic_data2_50 = self.Automatic_data2[-50:]

        self.draw_chart2()

    def draw_chart2(self):
        # 清除旧的数据系列
        self.chart2.removeAllSeries()

        # 创建数据系列
        series2 = QLineSeries()

        # 添加数据点
        for point in self.Automatic_data2_50:
            series2.append(point[0], point[1])

        # 添加数据系列到图表上
        self.chart2.addSeries(series2)

        # 获取数据中的最大最小值
        x_values2 = [point[0] for point in self.Automatic_data2_50]
        y_values2 = [point[1] for point in self.Automatic_data2_50]

        # 设置坐标轴的范围（加上一些边界空间）
        self.axis_x2.setRange(0, max(x_values2))
        self.axis_y2.setRange(0, max(y_values2))

        # 更新图表
        self.Automatic_chart_view2.setChart(self.chart2)


    def net_connect(self):
        if self.TCP_client:
            pass
        else:
            self.TCP_start_th = threading.Thread(target=self.TCP_start, args=())
            self.TCP_init_start_th = threading.Thread(target=self.TCP_init_start, args=())
            self.TCP_start_th.start()
            self.TCP_init_start_th.start()

    def TCP_start(self):
        self.TCP_client = NetWorkBase.network_TCP('192.168.0.100', 2222, 8001)

    def TCP_init_start(self):
        self.TCP_init = NetWorkBase.network_TCP('192.168.0.100', 3333, 8000)
        self.TCP_init.netsend("$II,201-2,1,301,10,1,2,3,11:30:56,*&&")
        self.TCP_init.netsend("$IR,148,1,1,"
                              "2-401,2,0,0,0,0,00.00,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,"
                              "0,0,0,11:30:56,*&&")

        init_str = "$SV,141,U-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,2,3,11:30:56,*&&"
        self.state.info_flash(init_str)

    def net_send(self):
        self.E_V = float(self.V_edit.text())
        self.E_A = float(self.A_edit.text())
        self.E_D = float(self.D_edit.text())

        self.flag = True
        if self.TCP_client:
            self.send_th = threading.Thread(target=self.sendMessage, args=())
            self.send_th.start()

    def stop_send(self):
        self.flag = False

    def flash_PID(self):

        self.control_dic['a_p'] = float(self.KP_edit.text())
        self.control_dic['a_d'] = float(self.KD_edit.text())
        self.PID_A.control_dic = self.control_dic
        print("self.control_dic['a_p']:", self.PID_A.control_dic['a_p'])
        print("self.control_dic['a_d']:", self.PID_A.control_dic['a_d'])
        self.PID_A.set_pid()
        self.PID_A.PID_clear()

    def test(self):
        N = self.PID_A.calculate(1, 1, 1)
        print(N)

    def enter_V_A(self):
        self.E_A = float(self.A_edit.text())
        self.E_V = float(self.V_edit.text())
        self.E_D = float(self.D_edit.text())
        print("self.E_A:", self.E_A)
        print("self.E_V:", self.E_V)
        print("self.E_D:", self.E_D)

    def cul_info(self, V, A, D):
        F = self.PID_V.calculate(V, self.state.u, self.state.du)
        N = self.PID_A.calculate(A, self.state.yaw, self.state.r)
        N = self.Saturation(N, 60, -60)
        T = self.PID_D.calculate(D, self.state.z, self.state.w, self.state.roll, self.state.q)
        self.send_str = self.cul_str(F, N, T)

    def cul_str(self, F, N, T):
        self.new_time = datetime.datetime.now()
        self.new_time_0 = self.new_time.strftime('%Y-%m-%d %H:%M:%S.%f')
        temp_str = f"$VS,65,U-2,{F},00.00,00.00,00.00,{T},{N},1,2,3,{self.new_time_0},*&&"
        # print("temp_str:", temp_str)
        return temp_str

    def sendMessage(self):
        current_dir = os.path.dirname(__file__)
        data_file_path = os.path.join(current_dir, 'data', 'VSlog.txt')
        self.file = open(data_file_path, 'a')
        self.file.write("start\n")
        while self.flag:
            self.cul_info(self.E_V, self.E_A, self.E_D)
            self.TCP_client.netsend(self.send_str)
            print("self.send_str", self.send_str)
            self.file.write(f"{self.send_str}\n")
            time.sleep(0.1)

    def getMessage(self):
        while True:
            if self.TCP_client:
                USV_state = self.TCP_client.Q_1.get()
                self.state.info_flash(USV_state)
                self.state_signal.emit(USV_state)

    def getMessage_1(self):
        while True:
            if self.TCP_init:
                temp_2 = self.TCP_init.Q_1.get()

    def add_text(self, value):
        time_now_str = datetime.datetime.now()
        time_new_str_1 = time_now_str.strftime('%Y-%m-%d %H:%M:%S.%f')
        temp_str = time_new_str_1 + value
        self.TextBox.append(temp_str)
        self.TextBox_x.setText(str(self.state.x))
        self.TextBox_y.setText(str(self.state.y))
        self.TextBox_z.setText(str(self.state.z))
        self.TextBox_u.setText(str(self.state.u))
        self.TextBox_v.setText(str(self.state.v))
        self.TextBox_w.setText(str(self.state.w))
        self.TextBox_pitch.setText(str(self.state.pitch))
        self.TextBox_roll.setText(str(self.state.roll))
        self.TextBox_yaw.setText(str(self.state.yaw))
        self.TextBox_p.setText(str(self.state.p))
        self.TextBox_q.setText(str(self.state.q))
        self.TextBox_r.setText(str(self.state.r))
        self.TextBox_time.setText(str(self.state.time_now))

    def mes_cul(self, value):
        pass

    def Saturation(self, value, limit_up, limit_down):
        if value > limit_up:
            return limit_up
        elif value < limit_down:
            return limit_down
        else:
            return value

    def closeEvent(self, QCloseEvent):
        if self.TCP_client:
            self.TCP_client.stop_Run()
            self.TCP_init.stop_Run()
        self.flag = False


if __name__ == '__main__':
    app = QApplication(sys.argv)
    server = ServerApp()
    server.state_signal.connect(lambda value: server.add_text(value))
    server.show()
    sys.exit(app.exec_())

    tst_str = "$SV,141,U-3,4.00,5.00,0.00,1.00,2.00,3.00,1.00,2.00,3.00,1.00,2.00,3.00,1.00,2.00,3.00,1.00,2.00,3.00,35.756987,120.265478,1,2,3,11:30:56,*&&"
